/*  -------------------------- (C) COPYRIGHT 2022 Fortiortech ShenZhen ---------------------------*/
/**
 * @copyright (C) COPYRIGHT 2022 Fortiortech Shenzhen
 * @file      xxx.c
 * @author    Fortiortech  Appliction Team
 * @since     Create:2022-09-09
 * @date      Last modify:2022-09-14
 * @note      Last modify author is Leo.li
 * @brief
 */
#include <MyProject.h>


/**
 * @brief     保护处理函数，关闭FOC输出
 * @date      2022-07-14
 */	
void FaultProcess(void)
{
    ClrBit(DRV_CR, DRVEN);  //Driver Disable
    ClrBit(DRV_CR, FOCEN);  //FOC Disable
    MOE     = 0;
}


/**
 * @brief     过压欠压保护函数：程序每5ms判断一次，母线电压大于过压保护值时，计数器加一，计数器值超过20次，判断为过压保护，关闭输出;反之，计数器慢慢减
              同理，欠压保护。
              电机过欠压保护状态下，母线电压恢复到欠压恢复值以上，过压恢复值以下时，计数器加一，超过200次后，恢复。
 * @date      2022-07-14
 */
void Fault_OverUnderVoltage(void)
{
    if (mcFaultSource == FaultNoSource) //程序无其他保护下
    {
        //过压保护
        if (mcFocCtrl.mcDcbusFlt > OVER_PROTECT_VALUE)   //母线电压大于过压保护值时，计数，超过20次，判断为过压保护，关闭输出;反之，计数器慢慢减
        {
            mcFaultDect.OverVoltDetecCnt++;
            
            if (mcFaultDect.OverVoltDetecCnt > 10) //检测50ms
            {
                mcFaultDect.OverVoltDetecCnt = 0;
                mcFaultSource             = FaultOverVoltage;
            }
        }
        else if (mcFaultDect.OverVoltDetecCnt > 0)
        {
            mcFaultDect.OverVoltDetecCnt--;
        }
        
        //欠压保护
        if (mcFocCtrl.mcDcbusFlt < UNDER_PROTECT_VALUE)
        {
            mcFaultDect.UnderVoltDetecCnt++;
            
            if (mcFaultDect.UnderVoltDetecCnt > 50) //检测50ms
            {
                mcFaultDect.UnderVoltDetecCnt = 0;
                mcFaultSource              = FaultUnderVoltage;
            }
        }
        else if (mcFaultDect.UnderVoltDetecCnt > 0)
        {
            mcFaultDect.UnderVoltDetecCnt--;
        }
    }
    
    #if ((SPEED_MODE != ONOFFTEST)&&(VoltageRecoverEnable))
    {
        /*******过压欠压保护恢复*********/
        if ((mcState == mcFault) && ((mcFaultSource == FaultUnderVoltage) || (mcFaultSource == FaultOverVoltage)))
        {
            if ((mcFocCtrl.mcDcbusFlt < OVER_RECOVER_VALUE) && (mcFocCtrl.mcDcbusFlt > UNDER_RECOVER_VALUE) && (mcProtectTime.VoltagePretectTimes < VoltageRecoverTimes))
            {
                mcFaultDect.VoltRecoverCnt++;
                
                if (mcFaultDect.VoltRecoverCnt > VoltageRecoverTime) //连续检测1s，若正常则恢复
                {
                    mcFaultSource = FaultNoSource;
                    mcFaultDect.VoltRecoverCnt = 0;
                }
            }
            else
            {             
                mcFaultDect.VoltRecoverCnt = 0;
            }
        }
    }
    #endif
}

/**
 * @brief     电机运行或者启动时，当三相中某一相最大值大于OverCurrentValue，则OverCurCnt加1
 * @date      2022-07-14
 */
void Fault_OverCurrent(void)
{
    if ((mcState == mcRun) || (mcState == mcStart))                       // check over current in rum and open mode
    {
        Atan_Us_MDU(FOC__IA, FOC__IBET, mcCurVarible.Is);
        
        if (mcCurVarible.Is >= OverSoftCurrentValue)
        {
            mcCurVarible.OverCurCnt++;
            
            if (mcCurVarible.OverCurCnt >= OverSoftCurrent_DectTime)
            {
                mcFaultSource     = FaultSoftOVCurrent;
                mcCurVarible.OverCurCnt = 0;
            }
        }
        else if (mcCurVarible.OverCurCnt > 0)
        {
            mcCurVarible.OverCurCnt--;
        }
    }
	
	#if ((SPEED_MODE != ONOFFTEST)&&(CurrentRecoverEnable))      //过流保护恢复使能
    {
        Fault_OverCurrentRecover();
    }
    #endif
}

/**
 * @brief     软硬件过流保护恢复
 * @date      2022-07-14
 */
void Fault_OverCurrentRecover(void)
{
    if ((mcState == mcFault) && ((mcFaultSource == FaultSoftOVCurrent) || (mcFaultSource == FaultHardOVCurrent)) && (mcProtectTime.CurrentPretectTimes < 5))
    {
        mcFaultDect.CurrentRecoverCnt++;
        
        if (mcFaultDect.CurrentRecoverCnt >= OverCurrentRecoverTime) //1000*5=5s
        {
            mcFaultDect.CurrentRecoverCnt = 0;
            mcProtectTime.CurrentPretectTimes++;
            mcFaultSource = FaultNoSource;
        }
    }
}

/**
 * @brief     启动检测
 * @date      2022-07-14
 */
void Fault_Start(void)
{
    if (mcState == mcRun)
    {
        //方法一，速度大于最大速度，同时反电动势值低于一定值
        if (mcFaultDect.StartSpeedCnt <= 600)
        {
            mcFaultDect.StartSpeedCnt++;
            
            //方法一
            if (mcFocCtrl.SpeedFlt > (uint16)(mcFocCtrl.EsValue * 3))
            {
                mcFaultDect.StartSpeedAndEsCnt++;
                
                if (mcFaultDect.StartSpeedAndEsCnt >= 30)
                {
                    mcFaultDect.StartSpeedAndEsCnt = 0;
                    mcFaultSource         = FaultStart;
                    mcProtectTime.SecondStartTimes++;
                    mcProtectTime.StartFlag = 1;
                }
            }
            else
            {
                mcFaultDect.StartSpeedAndEsCnt = 0;
            }
        }
        
        //方法二
        if (mcFaultDect.StartEsCnt <= 1000) //前5s，等待1.0s后，开始判断ES，如果超过一定次数，则失败
        {
            mcFaultDect.StartEsCnt++;
            mcFaultDect.StartDelay++;
            
            if (mcFaultDect.StartDelay >= 300)               // 1.5s
            {
                mcFaultDect.StartDelay = 300;
                
                if ((mcFocCtrl.EsValue < 200))
                {
                    mcFaultDect.StartESCount++;
                    
                    if (mcFaultDect.StartESCount >= 10)
                    {
                        mcFaultSource = FaultStart;
                        mcProtectTime.SecondStartTimes++;
                        mcFaultDect.StartDelay     = 0;
                        mcFaultDect.StartESCount   = 0;
                        mcProtectTime.StartFlag = 2;
                    }
                }
                else
                {
                    if (mcFaultDect.StartESCount > 0)
                    {
                        mcFaultDect.StartESCount--;
                    }
                }
            }
        }
        else
        {
            mcFaultDect.StartESCount = 0;
        }
        
        //方法三，长时间在CtrlMode=0状态
        if (mcFocCtrl.CtrlMode == 0)        //
        {
            mcFaultDect.StartFocmode++;
            
            if (mcFaultDect.StartFocmode >= 600)
            {
                mcFaultDect.StartFocmode = 0;
                mcFaultSource         = FaultStart;
                mcProtectTime.SecondStartTimes++;
                mcProtectTime.StartFlag = 3;
            }
        }
    }
    
    //起动保护恢复
    #if ((SPEED_MODE != ONOFFTEST)&&(StartRecoverEnable))    //起停测试时屏蔽起动保护恢复
    {
        if ((mcFaultSource == FaultStart) && (mcState == mcFault) && (mcProtectTime.SecondStartTimes <= StartRecoverTimes))
        {
            mcFaultDect.StartRecoverDelayCnt++;
            
            if (mcFaultDect.StartRecoverDelayCnt > StartRecoverTime)  //启动保护延时恢复时间
            {
                mcFaultDect.StartRecoverDelayCnt = 0;
                mcFaultSource = FaultNoSource;
            }
        }
    }
    #endif
}

/**
 * @brief     堵转检测
 * @date      2022-07-14
 */
void Fault_Stall(void)
{
    if (mcState == mcRun)
    {
        if (mcFaultDect.StallDelayCnt <= 800) //5s
        {
            mcFaultDect.StallDelayCnt++;
        }
        else
        {
            //method 1，判断反电动势太小或当反电动势太小，转速太大
            if ((mcFocCtrl.EsValue < 400) || ((FOC__EOME > _Q15(1250.0 / MOTOR_SPEED_BASE)) && (mcFocCtrl.EsValue < 600)))
            {
                mcFaultDect.StallDectEs++;
                
                if (mcFaultDect.StallDectEs >= 50)
                {
                    mcFaultDect.StallDectEs = 0;
                    mcFaultSource        = FaultStall;
                    mcProtectTime.StallFlag = 1;
                }
            }
            else
            {
                if ( mcFaultDect.StallDectEs > 0)
                {
                    mcFaultDect.StallDectEs--;
                }
            }
            
            //method 2，判断速度低于堵转最小值或者超过堵转最大值
            if ((mcFocCtrl.SpeedFlt < MOTOR_SPEED_STAL_MIN_RPM) || (mcFocCtrl.SpeedFlt > MOTOR_SPEED_STAL_MAX_RPM))
            {
                mcFaultDect.StallDectSpeed++;
                
                if (mcFaultDect.StallDectSpeed >= 30)
                {
                    mcFaultDect.StallDectSpeed = 0;
                    mcFaultSource           = FaultStall;
                    mcProtectTime.StallFlag = 2;
                }
            }
            else
            {
                if (mcFaultDect.StallDectSpeed > 0)
                {
                    mcFaultDect.StallDectSpeed--;
                }
            }
            
			//method 2，判断速度低于堵转最小值或者超过堵转最大值
            if (mcFocCtrl.SpeedFlt > (uint16)(mcFocCtrl.EsValue * 3))
            {
                mcFaultDect.StallSpeedAndEsCnt++;
                
                if (mcFaultDect.StallSpeedAndEsCnt >= 50)
                {
                    mcFaultDect.StallSpeedAndEsCnt = 0;
                    mcFaultSource         = FaultStall;
                    mcProtectTime.StallFlag = 3;
                }
            }
            else
            {
                if (mcFaultDect.StallSpeedAndEsCnt > 0)
                { mcFaultDect.StallSpeedAndEsCnt--; }
            }
        }
    }
    
    #if ((SPEED_MODE != ONOFFTEST)&&(StallRecoverEnable))
    {
        /*******堵转保护恢复*********/
        if ((mcFaultSource == FaultStall) && (mcState == mcFault) && (mcProtectTime.StallTimes <= StallRecoverTimes)) //堵转重启次数
        {
            mcFaultDect.StallReCount++;
            
            if (mcFaultDect.StallReCount >= StallRecoverTime)
            {
				mcProtectTime.StallTimes ++;
                mcFaultDect.StallReCount = 0;
                mcFaultSource = FaultNoSource;
            }
        }
        else
        {
            mcFaultDect.StallReCount = 0;
        }
    }
    #endif
}

/**
 * @brief     缺相保护检测
 * @date      2022-07-14
 */
void Fault_PhaseLoss(void)
{
    if (mcState == mcRun)
    {
        if (mcFaultDect.DectCycleCnt < 500)
        {
            mcFaultDect.DectCycleCnt++;
        }
        else
        {
            mcCurVarible.Max_ia = FOC__IAMAX;
            mcCurVarible.Max_ib = FOC__IBMAX;
            mcCurVarible.Max_ic = FOC__ICMAX;
            
            if (((mcCurVarible.Max_ia > (mcCurVarible.Max_ib << 1)) || (mcCurVarible.Max_ia > (mcCurVarible.Max_ic << 1)))
                && (mcCurVarible.Max_ia > PhaseLossCurrentValue))
            {
                mcFaultDect.AOpencnt++;
            }
            else
            {
                if (mcFaultDect.AOpencnt > 0)
                {
                    mcFaultDect.AOpencnt --;
                }
            }
            
            if (((mcCurVarible.Max_ib > (mcCurVarible.Max_ia << 1)) || (mcCurVarible.Max_ib > (mcCurVarible.Max_ic << 1)))
                && (mcCurVarible.Max_ib > PhaseLossCurrentValue))
            {
                mcFaultDect.BOpencnt++;
            }
            else
            {
                if (mcFaultDect.BOpencnt > 0)
                {
                    mcFaultDect.BOpencnt --;
                }
            }
            
            if (((mcCurVarible.Max_ic > (mcCurVarible.Max_ia << 1)) || (mcCurVarible.Max_ic > (mcCurVarible.Max_ib << 1)))
                && (mcCurVarible.Max_ic > PhaseLossCurrentValue))
            {
                mcFaultDect.COpencnt++;
            }
            else
            {
                if (mcFaultDect.COpencnt > 0)
                {
                    mcFaultDect.COpencnt --;
                }
            }
            
            mcCurVarible.Max_ia = 0;
            mcCurVarible.Max_ib = 0;
            mcCurVarible.Max_ic = 0;
            SetBit(FOC_CR2, ICLR);
            
            if ((mcFaultDect.AOpencnt > 20) || (mcFaultDect.BOpencnt > 20) || (mcFaultDect.COpencnt > 20))
            {
				mcProtectTime.LossPHTimes++;
                mcFaultSource = FaultLossPhase;
            }
        }
    }
    
    /*******缺相保护恢复*********/
    #if ((SPEED_MODE != ONOFFTEST)&&(PhaseLossRecoverEnable))           //起停测试时屏蔽缺相保护恢复
    {
        if ((mcFaultSource == FaultLossPhase) && (mcState == mcFault) && (mcProtectTime.LossPHTimes < PhaseLossRecoverTimes)) //可重启5次
        {
            mcFaultDect.mcLossPHRecCount++;
            
            if (mcFaultDect.mcLossPHRecCount >= PhaseLossRecoverTime)
            {
                mcFaultDect.AOpencnt = 0;
                mcFaultDect.BOpencnt = 0;
                mcFaultDect.COpencnt = 0;
                mcFaultSource = FaultNoSource;
            }
        }
        else
        {
            mcFaultDect.mcLossPHRecCount = 0;
        }
    }
    #endif
}

/**
 * @brief     保护函数汇总处理 
 * @date      2022-07-14
 */		
void Fault_Detection(void)
{
	#if (OverSoftCurrentProtectEnable)      //过流保护使能
	{        
        Fault_OverCurrent();                //软件过流保护
    }
	#endif
    #if (VoltageProtectEnable == 1)         //过压保护使能
    {
        Fault_OverUnderVoltage();
    }
    #endif
    #if (StartProtectEnable == 1)           //启动保护使能
    {
        Fault_Start();
    }
    #endif
    #if (StallProtectEnable == 1)           //堵转保护使能
    {
        Fault_Stall();
    }
    #endif
    #if (PhaseLossProtectEnable == 1)        //缺相保护使能
    {
        Fault_PhaseLoss();
    }
    #endif
}